#!/usr/bin/env python
PACKAGE = "mrs_uav_controllers"

import roslib;
roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

horizontal = gen.add_group("Horizontal gains");

horizontal.add("kpxy", double_t, 0, "Position constant for xy-axes", 0.0, 0.0, 40.0)
horizontal.add("kvxy", double_t, 0, "Velocity constant for xy-axes", 0.0, 0.0, 40.0)
horizontal.add("kaxy", double_t, 0, "Acceleration constant for xy-axes", 0.0, 0.0, 2.0)
horizontal.add("kiwxy", double_t, 0, "Integral constant for xy-axes", 0.0, 0.0, 10.0)
horizontal.add("kibxy", double_t, 0, "Integral constant for xy-axes", 0.0, 0.0, 10.0)
horizontal.add("kiwxy_lim", double_t, 0, "xy-axes integral limit", 0.0, 0.0, 10.0)
horizontal.add("kibxy_lim", double_t, 0, "xy-axes integral limit", 0.0, 0.0, 10.0)

vertical = gen.add_group("Vertical gains");

vertical.add("kpz", double_t, 0, "Position constant for z-axis", 0.0, 0.0, 200.0)
vertical.add("kvz", double_t, 0, "Velocity constant for z-axis", 0.0, 0.0, 200.0)
vertical.add("kaz", double_t, 0, "Acceleration constant for z-axis", 0.0, 0.0, 2.0)

attitude = gen.add_group("Attitude control");

attitude.add("kq_roll_pitch", double_t, 0, "Attitude constant for intrinsic roll and pitch control", 0.0, 0.0, 20.0)
attitude.add("kq_yaw", double_t, 0, "Attitude constant for intrinsic yaw control", 0.0, 0.0, 40.0)

attitude = gen.add_group("Mass estimator");

attitude.add("km", double_t, 0, "Integral constant for mass", 0.0, 0.0, 2.0)
attitude.add("km_lim", double_t, 0, "Mass integral limit", 0.0, 0.0, 50.0)

output = gen.add_group("Output");

preffered_mode = gen.enum([
                            gen.const("desired_actuators",     int_t, 0, "Actuators"),
                            gen.const("desired_control_group", int_t, 1, "Control group"),
                            gen.const("desired_attitude_rate", int_t, 2, "Attitude rate"),
                            gen.const("desired_orientation",   int_t, 3, "Attitude"),
                          ], "Preferred output mode")

rotation_enum = gen.enum([gen.const("lee", int_t, 0, "Lee rotation"),
                          gen.const("baca", int_t, 1, "Baca rotation")],
                          "Rotation type")

output.add("jerk_feedforward", bool_t, 0, "Jerk feedforward", True)
output.add("pitch_roll_heading_rate_compensation", bool_t, 0, "Pitch/Roll rate -> heading rate compensation", True)
output.add("rotation_type", int_t, 0, "Rotationtype", 0, 0, 1, edit_method=rotation_enum)
output.add("preferred_output_mode", int_t, 0, "Preffered output mode", 0, 0, 3, edit_method=preffered_mode)

exit(gen.generate(PACKAGE, "Se3Controller", "se3_controller"))
